#ifndef __TEST_HPP
#define __TEST_HPP

#include <main.h>
#include <math.h>

#define currentPID 0
#define speedPID 1
#define positionPID 2

//机器参数
#define L1 110
#define L2 110
#define H  110

typedef struct SbyXY{
	float ORG_X;
	float ORG_Y;

	float CACL_A;
	float CACL_B;

	float DeltaX;
	bool success;
};

typedef struct SbyPID{
    float kp,ki,kd;        
    float setpoint;
    float result;
    float integralValue;
    float errorabsmax;
    float errorabsmin;
    float derivative;
    float maximum;
    float minimum;
    float preerror;
    float lasterror;
    float alpha;
};

typedef struct SbyCurrentSetting{
    float CurrentBasicA;
    float CurrentBasicB;  

    float CurrentForceA;
    float CurrentForceB;

    float CurrentErrorA;
    float CurrentErrorB;
};

typedef struct CurrentData{
    float A;
    float B;
};

typedef struct BasicConfig{
    int BasicPwmData;
};

typedef struct EncoderData{
    float PositionA;
    float PositionB;

    float SpeedA;
    float SpeedB;

    int PositionWithoutTransformA;
    int PositionWithoutTransformB;
};

typedef struct EncoderSetting{
    float MinimumSize;
    float PositionError;
};

extern volatile CurrentData currentdata;
extern volatile EncoderData encoderdata;

class SbyIsMySon{

    public:

    SbyIsMySon();
    ~SbyIsMySon();

    void Excute(void);
    void ExcuteInXY(int x,int y);
    void UpdateConfig(int PIDtype,SbyPID sbypid,SbyCurrentSetting sbycurrentsetting,EncoderSetting encodersetting,BasicConfig basicconfig);

    private:

    float VariableIntegralCoefficient(float error,float absmax,float absmin);
    float PIDRegulator(float setpoint,float currpoint);
    void MotorControl(int16_t ch,int16_t pwm);
    float CurrentAndPositionControl(float pos);
    int GetCurrentControlState(float pos);
    bool CurrentControl(int state,float pos);
    bool ObjectMoveRightInCurrentMode(float pos);
    bool ObjectMoveLeftInCurrentMode(float pos);
    bool ObjectMoveRightInEncoderMode(float pos);
    bool ObjectMoveLeftInEncoderMode(float pos);
    bool PositionControl(int state,float pos);
    void TestCurrent(float current);
    float PositionWithoutCurrentControl(float pos);
    void TestSpeed(float speed);
    float PulseToMicrometer(uint32_t pulse,int T);
    uint32_t MicrometerToPulse(float mm,int T);
    bool GoToPosition(float mmA,float mmB);
    SbyXY InverseKinematics(float x,float y);
    SbyXY BinaryEquationSolver(float a,float b,float c);
    void LinearInterpolator(int x1,int y1,int x2,int y2);
    void TestKinematics(void);
    void Function1(void);
    void Function2(void);
	void Function3(void);

    SbyPID currentpiddata;
    SbyPID speedpiddata_A;
    SbyPID speedpiddata_B;

    SbyPID sbypiddata;

    SbyCurrentSetting sbycurrentsettingdata;
    EncoderSetting encodersettingdata;
    BasicConfig basicconfigdata;

};


#endif
